#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "../../cvblob_0_10_4/cvblob.h"
#include "../../Common/HsvImage.h"

#include <iostream>
#include <ctime>

#pragma comment(lib, "opencv_core242d")
#pragma comment(lib, "opencv_imgproc242d")
#pragma comment(lib, "opencv_highgui242d")

using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
	clock_t start, duration;

	// string filename = "../../InputFiles/Video/red_objects.mp4";
	string filename = "../../InputFiles/Video/color_ring.mp4";

	VideoCapture cap;

	cap.open(filename);

	if( !cap.isOpened() )
	{
		printf("can not open camera or video file/n");
		return -1;
	}

	Mat frame;
	int framesPerSecond		 = (int)cap.get(CV_CAP_PROP_FPS);
	int DelayBeforeNextFrame = 1000 / framesPerSecond;
	int restTimeBeforeNextFrame = 1;

	Mat erodeElement = getStructuringElement(MORPH_ERODE,  Size(3, 3), Point(1, 1)); 
	
	Mat hsvImage, binaryFrame, invBinaryFrame, hsvFrameChannels[3], result;
	Mat hue1, hue2, hueResult, binary_tmp, saturation;

	cvb::CvBlobs  blobs;
	cvb::CvTracks tracks;

	while(cap.read(frame))
	{
		start = clock();
		HsvImage hsvImg(frame);

		// imshow("Hue", hsvImg.Hue());
		// imshow("Sat", hsvImg.Saturation());

		threshold(hsvImg.Hue(), hue1, 170, 255, THRESH_BINARY);		// upper hues from purple to red
		threshold(hsvImg.Hue(), hue2, 15, 255, THRESH_BINARY_INV);	// lower hues from red to orange
		threshold(hsvImg.Saturation(), saturation, 80, 255, THRESH_BINARY);

		hueResult = hue1 + hue2;
		result = saturation.mul(hueResult);

		erode(result, result, erodeElement);
		imshow("Red color mask", result);

		IplImage ipl_tmp = result;
		IplImage ipl_src = frame;

		IplImage *labelImg	= cvCreateImage(cvGetSize(&ipl_tmp), IPL_DEPTH_LABEL, 1);
		IplImage *blobImg	= cvCreateImage(cvGetSize(&ipl_tmp), IPL_DEPTH_8U, 3);

		unsigned int iResult = cvb::cvLabel(&ipl_tmp, labelImg, blobs);

		cvb::cvFilterByArea(blobs, 800u, 100000u);
		// cvb::cvRenderBlobs(labelImg, blobs, &ipl_src, &ipl_src, CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_BOUNDING_BOX);

		cvUpdateTracks(blobs, tracks, 10., 2);
		cvRenderTracks(tracks, &ipl_src, &ipl_src, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX);

		cvReleaseBlobs(blobs);
		cvReleaseTracks(tracks);
		cvReleaseImage(&labelImg);
		cvReleaseImage(&blobImg);

		duration = clock() - start;
		restTimeBeforeNextFrame = DelayBeforeNextFrame > (duration) ? DelayBeforeNextFrame - duration: 1;

		ostringstream text;
		text << "FPS: " << 1000 / duration;
		Mat imageToShow = cvarrToMat(&ipl_src);
		putText(imageToShow, text.str(), Point(10,20), FONT_HERSHEY_PLAIN, 1.5, CV_RGB(255, 255, 255), 3);
		putText(imageToShow, text.str(), Point(10,20), FONT_HERSHEY_PLAIN, 1.5, CV_RGB(0, 0, 0), 1);
		
		imshow("Tracked objects", imageToShow);

		int key = waitKey( restTimeBeforeNextFrame );
		if(key == 27)
			break;
	}

	return 0;
}
